;;; 
;;; Vuty: Vetorized Operation Utilities
;;; 
BITS 64
;;; 
;;;  LICENSE AGREEMENT:
;;; 
;;;  You agree that we don't agree to anything.
;;; 
;;; 
;;;  DISCLAIMER OF WARRANTIES:
;;; 
;;;  This software is provided to you "AS IS," and the authors and
;;;  contributors disclaim any warranty or liability obligations to you
;;;  of any kind, implied or express, including but not limited to the
;;;  warranties of merchantability, fitness for a particular purpose,
;;;  and non-infringement.
;;; 
section .data align=16
funity dd 1.0,1.0,1.0,1.0
;; lower threshold for mulps
floth  dd 1.0e-16, 1.0e-16, 1.0e-16, 1.0e-16
mxcsrd dd 0
mx dq 0
nh dq 0
nullqd dd 0,0,0,0
;;;
section .text align=16
%assign rc   0x04
;;;
global initenvv_
global addps_
global muladdpsl_
global muladdps_
global muladdpsl2_
global muladdps2_
global divps_
global muldivps_
global get_cpuid_
global fxsave_
global setdaz_
global getn_
;;;
initenvv_:	
	push rbp
	mov rbp, rsp
	;;
	mov [mx], rdi		; 1st mx
	mov [nh], rsi		; 2nd nh
	;;
	pop rbp
	ret
;;; 
%macro ld8s 1
%if %1 == 0
	movsd xmm0, [rsi+8*rax]	;
	movsd xmm5, [rsi+8*rax+8]
	movsd xmm4, [rsi+8*rax+16]
	movsd xmm7, [rsi+8*rax+24]
	unpcklpd xmm0, xmm5
	unpcklpd xmm4, xmm7
%else
  	movaps xmm0, [rsi+8*rax]	; a
  	movaps xmm4, [rsi+8*rax+16]	; a 	
%endif
	;; check if a=0
;;; test 20100405
movaps xmm2, xmm0	;
movaps xmm6, xmm4	;
cmpss xmm2, [nullqd], 4	;
cmpss xmm6, [nullqd], 4
pmovmskb r10, xmm2
pmovmskb r11, xmm6
add r10, r11
;;; end test
%endm
;;;
%macro ld8d 1
%if %1 == 0
	movsd xmm2, [rdi+8*rax]	
	movsd xmm5, [rdi+8*rax+8]
	movsd xmm6, [rdi+8*rax+16]
	movsd xmm7, [rdi+8*rax+24]
	unpcklpd xmm2, xmm5
	unpcklpd xmm6, xmm7
%else
  	movaps xmm2, [rdi+8*rax]	; b
 	movaps xmm6, [rdi+8*rax+16]	; b
%endif
%endm
;;;
%macro st8d 1
%if %1 == 0
	movsd [rdi+8*rax], xmm2
	movsd [rdi+8*rax+16], xmm6
	unpckhpd xmm2, xmm2
	unpckhpd xmm6, xmm6
	movsd [rdi+8*rax+8], xmm2
	movsd [rdi+8*rax+24], xmm6	
%else
    	movaps [rdi+8*rax], xmm2	; return
    	movaps [rdi+8*rax+16], xmm6	; return
%endif	
%endm
;;;
%macro doma8 2
 	ld8s %1
;;; TEST 20100405
	test r10, r10
	jz %%.donema8
;;; END TEST	
 	ld8d %2
  	mulps xmm0, xmm1	; a=a*w
 	mulps xmm4, xmm3	; a=a*w
  	addps xmm2, xmm0	; b=b+a
  	addps xmm6, xmm4	; b=b+a
 	st8d %2
%%.donema8:
	add rax, 4
	sub rcx, 1
%endm
;;;
%macro ld4s 1
%if %1 == 0
	movsd xmm0, [rsi+8*rax]
	movsd xmm5, [rsi+8*rax+8]
	unpcklpd xmm0, xmm5	;
%else
  	movaps xmm0, [rsi+8*rax]	; a
%endif
;;; test 20100427
movaps xmm2, xmm0		;
cmpss xmm2, [nullqd], 4		;
pmovmskb r10, xmm2		;
;;; end test
%endm
;;;
%macro ld4d 1
%if %1 == 0
	movsd xmm2, [rdi+8*rax]
	movsd xmm5, [rdi+8*rax+8]
	unpcklpd xmm2, xmm5
%else
  	movaps xmm2, [rdi+8*rax]	; b
%endif
%endm
;;;
%macro st4d 1
%if %1 == 0
	movsd [rdi+8*rax], xmm2
	unpckhpd xmm2, xmm2
	movsd [rdi+8*rax+8], xmm2 ;
%else
    	movaps [rdi+8*rax], xmm2	; return
%endif
%endm
;;;
%macro doma4 2
	ld4s %1
	test r10, r10
	jz %%.donema4
;;; remove 20100427
;$	or eax, eax
;$	movq rax, xmm5
;$;;; 	jz %%.donema4
;;; end remove
	ld4d %2	
  	mulps xmm0, xmm1	; a=a*w
  	addps xmm2, xmm0	; b=b+a
	st4d %2
%%.donema4:
	add rax, 2
	sub rcx, 1
%endm
;;; 
%macro ldsome2 4
;;; dst1, src1, dst2, src2
;;; destroy: xmm5, xmm7
	cmp rcx, 1
	jne %%.ld2
	movss %1, [%2]
	movss %3, [%4]
	jmp %%.ldw
%%.ld2:
	cmp rcx, 2
	jne %%.ld3
	movlps %1, [%2]
	movlps %3, [%4]
	jmp %%.ldw
%%.ld3:
	movlps %1, [%2]
	movlps %3, [%4]
	movss xmm5, [%2+8]
	movss xmm7, [%4+8]
	movlhps %1, xmm5
	movlhps %3, xmm7
%%.ldw
%endm
;;; 
%macro ldsome 2
;;; dst, src
;;; destroy: xmm5
	cmp rcx, 1
	jne %%.ld2
	movss %1, [%2]
	jmp %%.ldw
%%.ld2:
	cmp rcx, 2
	jne %%.ld3
	movlps %1, [%2]
	jmp %%.ldw
%%.ld3:
	movlps %1, [%2]
	movss xmm5, [%2+8]
	movlhps %1, xmm5
%%.ldw
%endm
;;; 
;;; subroutine addps(n,a,w,m,L)
;;; integer n
;;; real*4,intent(inout)::a(n,m),w
;;; integer m,L
;;; a=a+w
;;; 
addps_:
%assign p   0x18
%assign q   0x20
%assign L   0x28
%assign r   0x30
%assign buf 0x40
%assign src 0x48
;;; 
	push rbp
	mov rbp, rsp
	;;
  	mov [rbp-p], rsi	; p = ptr a(1,1)
 	mov rax, [rsi]		; rax = a(1,1)
  	mov [rbp-q], rax	;
	;; 
 	mov rax, [r8]		; L
  	mov [rbp-L], rax	; save L
	;; 
	mov r8, [rcx]		; m
	test r8, r8
	jz .exit		;
	;; 
	movss xmm1, dword [rdx]
	unpcklps xmm1, xmm1
	movlhps xmm1, xmm1
	;; 
	mov rcx, [rdi]		; n
	mov r9, rcx		;
	cmp rcx, 0		; n
	jl .px			;
.pxdone:	
	cmp r8, 0		; m
	jl .py			;
.pydone:
	mov rdx, rsi		; save ptr a
	;;
	shr rcx, 2		; rcx = n/4
	mov rax, r9		; n
	shl rcx, 2		; rcx = 4(n/4)
	sub rax, rcx		;
	mov [rbp-r], rax	; r = mod(n,4)
	mov rcx, r9		; rcx = n
	shr rcx, 2		;
	mov r9, rcx		; r9 holds n/4
	;; 
	test rcx, rcx
	jz .next		;
	;; 
.do:
	movups xmm0, [rsi]	; a
	addps xmm0, xmm1	; a=a+w
	movups [rsi], xmm0	; return
	add rsi, 16		;
	sub rcx, 1
	ja .do			;
	;;
.next:
	mov rcx, [rbp-r]	;
	test rcx, rcx
	jnz .addsome		;
.done:
	sub r8, 1
	jz .exit		;
  	mov rax, [rbp-L]	; rax = L
	lea rsi, [rdx+8*rax]	; skip one row
	mov rcx, r9		;
	mov rdx, rsi		; save ptr a
	test rcx, rcx
	jz .next		;
	jmp .do			;
.exit:
  	mov rsi, [rbp-p]	; restore orig
  	mov rax, [rbp-q]	;
 	mov [rsi], eax		;
	;; 
	pop rbp
	ret
	;; 
.addsome:
	mov [rbp-src], rsi	;
	ldsome xmm0, rsi	;
 	addps xmm0, xmm1	;
  	movups [rbp-buf], xmm0	;
 	mov rdi, rsi		;
 	lea rsi, [rbp-buf]	;
	cld			;
 	rep movsd		;
	mov rsi, [rbp-src]	;
	jmp .done		;
	;;
.px:
	neg rcx			; n = -n
	mov r9, rcx		; r9 holds |n|
	sub rcx, 1
	shl rcx, 2		; rcx = 4 * (|n|-1)
	sub rsi, rcx		; rsi = rsi-(|n|-1)*4
	mov rcx, r9		; rcx = |n|
	jmp .pxdone		;
	;; 
.py:
	neg r8			; r8 = |m|
	sub r8, 1
	mov rax, [rbp-L]	; rax = len
	mul r8			; rdx:rax = len * (|m| - 1)
	shl rax, 3		; rax = 8 * len * (|m| - 1)
	sub rsi, rax		; rsi = rsi - 8 * len * (|m| - 1)
	add r8, 1		; r8 = |m|
	jmp .pydone
;;;
;;; subroutine muladdps(n,pa,pw,m,pb)
;;; integer n,m
;;; real*4,intent(in)::w
;;; real*4,intent(in)::a(n,m)
;;; real*4,intent(out)::b(n,m)
;;; integer L=mx,K=nh
;;; b=b+a*w
;;; 
%macro muladdpsx 4		; 
%assign p    0x18
%assign q    0x20
%assign L    0x28
%assign r    0x30
%assign rr   0x38
%assign buf  0x40
%assign K    0x48
%assign rbx0 0x50
%assign n    0x58
%assign n4   0x60
;;; 
	push rbp
	mov rbp, rsp
	;;
	mov [rbp-rbx0], rbx	; save rbx
	;; 
	mov [rbp-p], r8
	mov rax, [r8]
  	mov [rbp-q], rax	;
	;; 
	mov r8, rcx		; m
	;; 
	movss xmm1, dword [rdx]
	unpcklps xmm1, xmm1
	movlhps xmm1, xmm1
	movaps xmm3, xmm1
	;; 
	mov rcx, rdi		; n
	;; 
	mov rdi, [rbp-p]	; rdi = ptr b
	cmp rcx, 0		; n
	jl %%.px			;
%%.pxdone:	
	mov [rbp-n], rcx	; save modified n
	cmp r8, 0		; m
	jl %%.py			;
%%.pydone:
	mov rdx, rsi		; save ptr a
	mov rbx, rdi		; save ptr b
	;;
	mov rcx, [rbp-n]	;
	mov rax, rcx		;
	shr rcx, 3		; rcx = n/8
	mov r9, rcx		; r9 holds n/8
	shl rcx, 3		; rcx = 8(n/8)
	sub rax, rcx		;
	mov [rbp-rr], rax	; rr = mod(n,8)
	;; 
	mov rcx, [rbp-rr]	;
	mov rax, rcx		;
	shr rcx, 2		; rcx = rr/4
	mov [rbp-n4], rcx	; n4 = rr/4
	shl rcx, 2		; rcx = 4(rr/4)
	sub rax, rcx		;
	mov [rbp-r], rax	; r = mod(rr,4)
;;;
%%.begin8:
	test r9, r9
 	jz %%.begin4		;
 	mov rcx, r9		; rcx = n/8
	;;
%%.for8:
%if %2 == 1
	mov rax, rsi
	test rax, 15
	jnz %%.for8u		; rsi is u
	mov rax, rdi
	and rax, 15
	jnz %%.for8au
%endif
%if %2 == 1
%%.do8aa:
	doma8 1, 1
	ja %%.do8aa
	jmp %%.done8
	;;
%%.for8au:
	xor rax, rax
%%.do8au:
 	doma8 1, 0
	ja %%.do8au
	jmp %%.done8
;;; 
%%.for8u:
	mov rax, rdi
	and rax, 15
	jnz %%.for8uu
%%.do8ua:
 	doma8 0, 1
	ja %%.do8ua
	jmp %%.done8
	;; 
%%.for8uu:
	xor rax, rax
%endif
;;; 
%%.do8uu:
 	doma8 0, 0
	ja %%.do8uu
;;; 
%%.done8:	
	lea rsi, [rsi+8*rax]	;
	lea rdi, [rdi+8*rax]	;
;;; 
%%.begin4:
	mov rcx, [rbp-n4]	;
	test rcx, rcx
	jz %%.next		;
	;;
%if %3 == 1
	mov rax, rsi
	test rax, 15
	jnz %%.for4u
	mov rax, rdi
	and rax, 15
	jnz %%.for4au
%endif
;;; 
%if %3 == 1 
%%.do4aa:
	doma4 1, 1		;
	ja %%.do4aa
	jmp %%.done4
%%.for4au:
	xor rax, rax
%%.do4au:
	doma4 1, 0		;
	ja %%.do4au
	jmp %%.done4
;;; 
%%.for4u:
	mov rax, rdi
	and rax, 15
	jnz %%.for4uu
%%.do4ua:
	doma4 0, 1		;
	ja %%.do4ua
	jmp %%.done4
	;;
%%.for4uu:
	xor rax, rax
%endif
;;; 
%%.do4uu:
	doma4 0, 0		;
	ja %%.do4uu
%%.done4:
	lea rsi, [rsi+8*rax]	;
	lea rdi, [rdi+8*rax]	;
	;;
%%.next:
	mov rcx, [rbp-r]	;
	test rcx, rcx
	jz %%.done		;
%%.addsome:
	mov [rbp-src], rsi	; save rsi
   	ldsome2 xmm0, rsi, xmm2, rdi
    	mulps xmm0, xmm1	; a=a*w
     	addps xmm2, xmm0	; b=b+a
	movsd [rbp-buf], xmm2
	unpckhpd xmm2, xmm2
	movsd [rbp-buf+8], xmm2
	;; 
	lea rsi, [rbp-buf]	;
 	cld			;
   	rep movsd		;
	mov rsi, [rbp-src]	; restore rsi
	;;
%%.done:
	sub r8, 1
	jz %%.exit		;
%if %1 == 2 
	mov rax, qword [nh]
	mov rcx, qword [mx]
  %if %4 == 0
	lea rsi, [rdx+8*rax]	; skip one row
  %else
 	lea rsi, [rdx+4*rax]	; no skip
  %endif
	lea rdi, [rbx+4*rcx]	;
%else
	mov rax, qword [mx]
	mov rcx, qword [nh]
	lea rsi, [rdx+4*rax]	;
  %if %4 == 0			;
	lea rdi, [rbx+8*rcx]	; skip one row
  %else
 	lea rdi, [rbx+4*rcx]	; no skip
  %endif
%endif
	mov rcx, r9		; restor n/8
	mov rdx, rsi		; save ptr a
	mov rbx, rdi		;
	test rcx, rcx
	jz %%.begin4		;
	jmp %%.for8		;
%%.exit:
  	mov rsi, [rbp-p]	; restore orig
  	mov rax, [rbp-q]	;
 	mov [rsi], eax		;
	;; 
%%.end:
	mov rbx, [rbp-rbx0]
	pop rbp
	ret
	;; 
%%.px:
	neg rcx			; n = -n
	mov r9, rcx		; r9 holds |n|
	sub rcx, 1
	shl rcx, 2		; rcx = 4 * (|n|-1)
	sub rsi, rcx		; rsi = rsi-(|n|-1)*4
	sub rdi, rcx		; rsi = rsi-(|n|-1)*4
	mov rcx, r9		; rcx = |n|
	jmp %%.pxdone
	;; 
%%.py:
	neg r8			; r8 = |m|
	sub r8, 1
	;; 
%if %1 == 2
	mov rax, qword [mx]
%else
	mov rax, qword [nh]
%endif
	mul r8			; rdx:rax = K * (|m| - 1)
	mov rcx, rax
	;;
%if %1 == 2
	mov rax, qword [nh]
%else
	mov rax, qword [mx]
%endif
	mul r8			; rdx:rax = L * (|m| - 1)
	;; 
%if %1 == 2
  %if %4 == 0
	shl rax, 3		; rax = 8 * L * (|m| - 1)
  %else
	shl rax, 2
  %endif
	shl rcx, 2		; rax = 4 * K * (|m| - 1)
%else
	shl rax, 2		;
  %if %4 == 0
	shl rcx, 3		; rax = 8 * K * (|m| - 1)
  %else
	shl rcx, 2
  %endif
%endif
	sub rsi, rax		; rsi = rsi - 8 * L * (|m| - 1)
	sub rdi, rcx		; rdi = rdi - 4 * K * (|m| - 1)
	;; 
	add r8, 1
	jmp %%.pydone		;
%endmacro
muladdpsl_:	
muladdpsx 2, 1, 1, 0
muladdps_:
muladdpsx 1, 1, 1, 0
muladdpsl2_:	
muladdpsx 2, 1, 1, 1
muladdps2_:
muladdpsx 1, 1, 1, 1
;;;
;;; subroutine divps(a,b,n,c)
;;; integer n
;;; real*4,intent(inout)::a(n)
;;; real*4,intent(in)::b(n)
;;; real*4,intent(in)::c(n)
;;; a=b/c
;;; where(c==0) a=0
;;; 
%macro divpsm 0
	movaps xmm1, [rax]	; c
	movaps xmm0, [rsi]	; b
	;; where c==0 c=1, b=0
	movapd xmm6, xmm1
	cmpps xmm6, xmm2, 4	; xmm6=(c/=0)
	andps xmm0, xmm6	; where c==0 b=0
	andnps xmm6, xmm3	; where c==0 xmm6=1
	addps xmm1, xmm6	; avoid 0div
   	divps xmm0, xmm1	; xmm0=b/c
%endmacro
;;; 
divps_:
%assign buf 0x30
;;; 
	push rbp
	mov rbp, rsp
	;;
	mov [rbp-buf], rcx	; save ptr c
	;; 
	mov rcx, rdx		; rcx = n
	mov rax, rcx		; rax = n
	shr rcx, 2		; rcx = n/4
	mov r8, rcx		; save n/4
	shl rcx, 2		; rcx = 4(n/4)
	sub rax, rcx		;
	mov rdx, rax		; rdx = mod(n,4)
	mov rcx, r8		; rcx = n/4
	;;
	mov rax, [rbp-buf]	; rax = ptr c
	;; 
	test rcx, rcx
	jz .next		;
	;;
	xorps xmm2, xmm2	; 0cle
	movapd xmm3, [funity]
.do:
	divpsm			;
	movaps [rdi], xmm0	; a=b/c
	add rdi, 16		;
	add rsi, 16		;
	add rax, 16		;
	sub rcx, 1
	ja .do			;
	;;
.next:
	mov rcx, rdx		; r
	test rcx, rcx
	jnz .divsome		;
	;; 
.exit:
	pop rbp
	ret
.divsome:
	divpsm			;
  	movaps [rbp-buf], xmm0	;
 	lea rsi, [rbp-buf]	;
   	rep movsd		;
	jmp .exit		;
;;;
;;; subroutine muldivps(a,b,n,c)
;;; integer n
;;; real*4,intent(inout)::a(n)
;;; real*4,intent(in)::b(n)
;;; real*4,intent(in)::c(n)
;;; where(c/=0) a=a*b/c
;;; 
%macro muldivpsm 1
;;; preserve xmm2, xmm3
;;; destroy xmm5, xmm7
;;; 0=full 1=some
%if %1 == 0
	movaps xmm0, [rsi]	; xmm0 = b
	movaps xmm1, [rax]	; xmm1 = c
 	movaps xmm4, [rdi]	; xmm4 = a
%else
	ldsome2 xmm0, rsi, xmm1, rax ;
	ldsome xmm4, rdi	     ;
%endif
	;; 
	;; where a<floth a=0
	movapd xmm6, xmm4
	cmpps xmm6, xmm2, 1	; xmm4=(a<floth)
	andnps xmm6, xmm4	; where a<floth xmm6=0
	movapd xmm4, xmm6	;
	;; 
	;; where c<floth c=1, a=0
	movapd xmm6, xmm1
	cmpps xmm6, xmm2, 1	; xmm6=(c<floth)
	movapd xmm5, xmm6	; xmm5=(c<floth)
 	andnps xmm6, xmm4	; where c<floth a=0
	andps xmm5, xmm3	; where c<floth xmm5=1.0
	addps xmm1, xmm5	; where c<floth c=1.0, avoid 0div
	movapd xmm4, xmm6	; xmm4 = a
	;; 
 	divps xmm0, xmm1	; xmm0 = b/c
  	mulps xmm0, xmm4	; xmm0 = b/c*a
%endmacro
;;; 
muldivps_:
%assign buf 0x30
;;; 
	push rbp
	mov rbp, rsp
	;;
	mov [rbp-buf], rcx	; save ptr c
	;; 
	mov rcx, rdx		; rcx = n
	mov rax, rcx		; rax = n
	shr rcx, 2		; rcx = n/4
	mov r8, rcx		; save n/4
	shl rcx, 2		; rcx = 4(n/4)
	sub rax, rcx		;
	mov rdx, rax		; rdx = mod(n,4)
	mov rcx, r8		; rcx = n/4
	;;
	mov rax, [rbp-buf]	; rax = ptr c
	;; 
	test rcx, rcx
	jz .next		;
	;;
	movaps xmm2, [floth]
	movaps xmm3, [funity]	;
.do:
	muldivpsm 0
	movaps [rdi], xmm0	; a=b/c
	add rdi, 16		;
	add rsi, 16		;
	add rax, 16		;
	sub rcx, 1
	ja .do			;
	;;
.next:
	mov rcx, rdx		; r
	test rcx, rcx
	jnz .muldivsome		;
	;; 
.exit:
	pop rbp
	ret
.muldivsome:
	muldivpsm 1		;
  	movaps [rbp-buf], xmm0	;
 	lea rsi, [rbp-buf]	;
   	rep movsd		;
	jmp .exit		;
;;;
;;; integer function  get_cpuid(buf,eax)
;;; integer*4,intent(out)::buf(4)
;;; integer,intent(in)::eax
;;; 
get_cpuid_:
	push rbp
	mov rbp, rsp
	push rbx
	;; 
	xor rdx, rdx		;
	pushfq			;eflags -> stack
	pop rax			; eax = eflags
	bt rax, 21		;
	jc .cle21		;
	;; 
	sub rdx, 1
	jmp .next		;
.cle21:
	add rdx, 1
.next:				
	xor rax, 0000000000200000h ;
	push rax		;
	popfq			;
	pushfq			;
	pop rax			;
	bt rax, 21		;
	;; 
	jc .elc21		;
	sub rdx, 1
	jmp .isnull		;
.elc21:
	add rdx, 1
.isnull:
	test rdx, rdx
	jnz .no			;
	;;
	mov rax, rsi		;
	cpuid			;
	;; 
	mov [rdi], eax		;
	mov [rdi+4], ebx	;
	mov [rdi+8], ecx	;
 	mov [rdi+12], edx	;
	;;
.done:
	xor rax, rax		;
.exit:
	pop rbx
	pop rbp
	ret
.no:
	xor rax, rax		;
	sub rax, 1
	jmp .exit		;
;;;
;;; subroutine fxsave(buf)
;;; integer*4,intent(in)::buf(128)
;;;
fxsave_:	
	push rbp
	mov rbp, rsp
	;;
	fxsave [rdi]
	;;
	pop rbp
	ret
;;;
;;; subroutien setdaz
;;;
setdaz_:	
	push rbp
	mov rbp, rsp
	;;
	stmxcsr [mxcsrd]
	mov eax, [mxcsrd]
	mov [rdi], eax
	or ax, 0940h		; DAZ,DM,UM
	mov [mxcsrd], eax
	ldmxcsr [mxcsrd]
	;;
	pop rbp
	ret
